/*
 * Copyright (C) 2021 HiHope Open Source Organization .
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *
 * limitations under the License.
 */

#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include <unistd.h>
#include <fcntl.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "robot_control.h"
#include "hi_adc.h"
#include "iot_errno.h"



#include "hi_wifi_api.h"
#include "lwip/netifapi.h"
#include "sockets.h"
#include "v2.0/common/mavlink.h"

#define     GPIO5   5
#define     FUNC_GPIO   0
#define     ADC_TEST_LENGTH             (20)
#define     VLT_MIN                     (100)
#define     OLED_FALG_ON                ((unsigned char)0x01)
#define     OLED_FALG_OFF               ((unsigned char)0x00)

osThreadId_t led_task_id;



#define TCP_SERVER_ADRESS    "183.230.64.58"    // 要连接TCP服务器地址
#define TCP_PORT             24558                // 要连接TCP服务器端口号
// #define TCP_SERVER_ADRESS    "10.31.200.167"    // 要连接TCP服务器地址
// #define TCP_PORT             6000                // 要连接TCP服务器端口号
//在sock_fd 进行监听，在 new_fd 接收新的链接
int sock_fd;
int ret;
int connectret;
int addr_length;
static const char *send_data = "Hello! I'm BearPi-HM_Nano TCP Client!\r\n";



int m_vehicleid = 107;
float x=0;
float y=0;


unsigned short  g_adc_buf[ADC_TEST_LENGTH] = { 0 };
unsigned short  g_gpio5_adc_buf[ADC_TEST_LENGTH] = { 0 };
unsigned int  g_gpio5_tick = 0;
unsigned char   g_car_control_mode = 0;
unsigned char   g_car_speed_control = 0;
unsigned int  g_car_control_demo_task_id = 0;
unsigned char   g_car_status = CAR_STOP_STATUS;
unsigned char   g_wifi_status =  CAR_WIFI_CONNECT_ING;

extern float GetDistance(void);
extern void trace_module(void);
extern void car_backward(void);
extern void car_forward(void);
extern void car_left(void);
extern void car_right(void);
extern void car_stop(void);
extern void engine_turn_left(void);
extern void engine_turn_right(void);
extern void regress_middle(void);

int setnonblockint(int fd) {
    int old_option = fcntl(fd, F_GETFL,0);
    int new_option = old_option | O_NONBLOCK;
    fcntl(fd, F_SETFL, new_option);
    return old_option;
}

void WifiLink(void)  
{
    static BOOL fgWifiConnected = FALSE;
			
    if(fgWifiConnected)   //防止重复连接WiFi
        return;

    printf("[WifiLink] Begin: fgWifiConnected[F]\n");
	
    //step 1: AT+STARTSTA   
    // #启动STA模式
    char ifname[WIFI_IFNAME_MAX_SIZE] = {0};  //“wlan0”
    int len = WIFI_IFNAME_MAX_SIZE;	

    if (HISI_OK != hi_wifi_sta_start(ifname, &len)) 
    {
        printf("[WifiLink] hi_wifi_sta_start fail\n");
        return;
    }

    //step 2: AT+CONN="SSID", ,2,"PASSWORD"
    //# 连接指定AP，其中SSID/PASSWORD为待连接的热点名称和密码
    hi_wifi_assoc_request request = {0};
    request.auth = HI_WIFI_SECURITY_WPA2PSK; //2

    char* ssid = "BIT_CQIC";          //Your SSID, HI_WIFI_MAX_SSID_LEN 32 Byte
    char* pswd = "bitcq190423";      //Your PSWD, HI_WIFI_MAX_KEY_LEN  64 Byte

    memcpy(request.ssid, ssid, strlen(ssid));
    memcpy(request.key, pswd, strlen(pswd));
    
    if (HISI_OK != hi_wifi_sta_connect(&request)) 
    {
        printf("[wifilink] hi_wifi_sta_connect fail\n");
        return;
    }

    //step 3: AT+DHCP=wlan0,1  
    //# 通过DHCP向AP请求wlan0的IP地址
    struct netif* p_netif = netifapi_netif_find(ifname);
    if(NULL == p_netif) 
    {
        printf("[WifiLink] netifapi_netif_find fail\n");
        return;
    }	
	
#if 1  //DHCP 自动分配IP
    if(HISI_OK != netifapi_dhcp_start(p_netif)) 
    {
        printf("[WifiLink] netifapi_dhcp_start fail\n");
        return;
    }	
#else  //设置固定 IP
    ip4_addr_t gw;
    ip4_addr_t ipaddr;
    ip4_addr_t netmask;
    IP4_ADDR(&gw,      192, 168,  1, 1);
    IP4_ADDR(&ipaddr,  192, 168,  1, 200);   //固定到这个 IP 
    IP4_ADDR(&netmask, 255, 255, 255, 0);

    if (HISI_OK != netifapi_netif_set_addr(p_netif, &ipaddr, &netmask, &gw)) 
    {
        printf("[WifiLink] netifapi_netif_set_addr fail\n");
        return;
    }

    if (HISI_OK != hi_wifi_start_connect()) 
    {
        printf("[WifiLink] hi_wifi_start_connect fail\n");
        return;
    }
#endif
	
    fgWifiConnected = TRUE;
    printf("[WifiLink] End.   fgWifiConnected[T]\n");

    return;
}


//MAVLINK ������
void _sendMessage(mavlink_message_t  message)
{
	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
	memset(buffer, 0, sizeof(buffer));
	 int len = mavlink_msg_to_send_buffer(buffer, &message);


	// 	//m_sendTcpScoket->flush();
	// 	//info->write((const char*)buffer, len);
	// 	/*m_sendTcpScoket->waitForBytesWritten(50);*/

	send(sock_fd, buffer, len, 0);
  
 
}

void packetheatbeat(void)
{
	

		mavlink_heartbeat_t  mavlink_heartbeat = { 0x00000000,0x0b,0x03,0x51,0x03,0x03 };
		//�����ڶ�λ0x0bΪ�ն�ģʽ�����ڿ�Լ��Ϊ����С�ӵĽ�ɫ���ͣ������ڶ���0x03��ʾϵͳ״̬ ����Լ��0x03 Ϊ����   0x03=4Ϊ����״̬��
		mavlink_message_t  msg;
		mavlink_msg_heartbeat_encode(m_vehicleid, 1, &msg, &mavlink_heartbeat);
		 _sendMessage(msg);
		
	

}

void packetattitude(void)
{


	
		mavlink_attitude_t attitude = { 0,1,1,1,0.0f,0.0f,0.0f };
		mavlink_message_t msg;
		mavlink_msg_attitude_encode(m_vehicleid, 0, &msg, &attitude);
		_sendMessage(msg);
	
	
}
void packetglobal_position(void)
{

		mavlink_global_position_int_t packet_in = {
			0,297303526,1065451096,600000,10000,0,0,0,0
		};
		//ע������ת��������λ��
		mavlink_message_t msg;
		uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
		memset(&buffer, 0, sizeof(buffer));
		mavlink_msg_global_position_int_encode(m_vehicleid, 0, &msg, &packet_in);
		_sendMessage(msg);	
}

void on_messageReceive(void)
{
  x=0;
  y=0;
  mavlink_status_t status;
  mavlink_message_t rmsg;
  
  uint8_t chan = MAVLINK_COMM_0;
	uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
  int c=0;
  memset(buffer, 0, sizeof(buffer));

  if(recv(sock_fd,buffer, MAVLINK_MAX_PACKET_LEN,0)==-1){
   //printf("%u",buffer[position]);recv(sock_fd, recvBuf, sizeof(recvBuf), 0)
  }
  else{
  
    for (int position = 0; position < sizeof(buffer); position++) {
      c=buffer[position];
    
      if(position== 10)
      {
        if(buffer[position]!=0){
            if(c==3){x=c/3.0;}
            if(c==253){x=(c-256)/3.0;}
        }
      }
      if(position== 12)
      {
        if(buffer[position]!=0){
        if(c==3){y=c/3.0;}
        if(c==253){y=(c-256)/3.0;}
        }
      }
      
    } 
    
  }

}


void switch_init(void)
{
    IoTGpioInit(5);
    hi_io_set_func(5, 0);
    IoTGpioSetDir(5, IOT_GPIO_DIR_IN);
    hi_io_set_pull(5, 1);

    IoTGpioInit(11);
    hi_io_set_func(11, 0);
    IoTGpioSetDir(11, IOT_GPIO_DIR_IN);
    hi_io_set_pull(11, 1);

    IoTGpioInit(12);
    hi_io_set_func(12, 0);
    IoTGpioSetDir(12, IOT_GPIO_DIR_IN);
    hi_io_set_pull(12, 1);
}

//按键中断响应函数
void gpio5_isr_func_mode(void)
{
    printf("gpio5_isr_func_mode start\n");
    unsigned int tick_interval = 0;
    unsigned int current_gpio5_tick = 0; 

    current_gpio5_tick = hi_get_tick();
    tick_interval = current_gpio5_tick - g_gpio5_tick;
    
    if (tick_interval < KEY_INTERRUPT_PROTECT_TIME) {  
        return NULL;
    }
    g_gpio5_tick = current_gpio5_tick;

    if (g_car_status == CAR_STOP_STATUS) {                
        g_car_status = CAR_TRACE_STATUS;                    //寻迹        
        printf("_______1______\n");
    } else if (g_car_status == CAR_TRACE_STATUS) {       
        g_car_status = CAR_STOP_STATUS; 
        printf("_______2______\n");
    } 
}

unsigned char get_gpio5_voltage(void *param)
{
    int i;
    unsigned short data;
    unsigned int ret;
    unsigned short vlt;
    float voltage;
    float vlt_max = 0;
    float vlt_min = VLT_MIN;

    hi_unref_param(param);
    memset_s(g_gpio5_adc_buf, sizeof(g_gpio5_adc_buf), 0x0, sizeof(g_gpio5_adc_buf));
    for (i = 0; i < ADC_TEST_LENGTH; i++) {
        ret = hi_adc_read(HI_ADC_CHANNEL_2, &data, HI_ADC_EQU_MODEL_4, HI_ADC_CUR_BAIS_DEFAULT, 0xF0); //ADC_Channal_2  自动识别模式  CNcomment:4次平均算法模式 CNend */
        if (ret != IOT_SUCCESS) {
            printf("ADC Read Fail\n");
            return  NULL;
        }    
        g_gpio5_adc_buf[i] = data;
    }

    for (i = 0; i < ADC_TEST_LENGTH; i++) {  
        vlt = g_gpio5_adc_buf[i]; 
        voltage = (float)vlt * 1.8 * 4 / 4096.0;  /* vlt * 1.8* 4 / 4096.0为将码字转换为电压 */
        vlt_max = (voltage > vlt_max) ? voltage : vlt_max;
        vlt_min = (voltage < vlt_min) ? voltage : vlt_min;
    }
    printf("vlt_max is %f\r\n", vlt_max);
    if (vlt_max > 0.6 && vlt_max < 1.0) {
        gpio5_isr_func_mode();
    } 
    else if(vlt_max > 1.0  &&  vlt_max < 2.0) 
    {
        return 1;
    }
    return 0;
}

//按键中断
void interrupt_monitor(void)
{
    unsigned int  ret = 0;
    /*gpio5 switch2 mode*/
    g_gpio5_tick = hi_get_tick();
    ret = IoTGpioRegisterIsrFunc(GPIO5, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_FALL_LEVEL_LOW, get_gpio5_voltage, NULL);
    if (ret == IOT_SUCCESS) {
        printf(" register gpio5\r\n");
    }
}

/*Judge steering gear*/
static unsigned int engine_go_where(void)
{
    float left_distance = 0;
    float right_distance = 0;
    /*舵机往左转动测量左边障碍物的距离*/

    engine_turn_left();
    hi_sleep(100);
    left_distance = GetDistance();
    hi_sleep(100);

    /*归中*/
    regress_middle();
    hi_sleep(100);

    /*舵机往右转动测量右边障碍物的距离*/
    engine_turn_right();
    hi_sleep(100);
    right_distance = GetDistance();
    hi_sleep(100);

    /*归中*/
    regress_middle();
    if (left_distance > right_distance) {
        return CAR_TURN_LEFT;
    } else {
        return CAR_TURN_RIGHT;
    }
}

/*根据障碍物的距离来判断小车的行走方向
1、距离大于等于20cm继续前进
2、距离小于20cm，先停止再后退0.5s,再继续进行测距,再进行判断
*/
/*Judge the direction of the car*/
static void car_where_to_go(float distance)
{
    if (distance < DISTANCE_BETWEEN_CAR_AND_OBSTACLE) {
        car_stop();
        hi_sleep(500);
        car_backward();
        hi_sleep(500);
        car_stop();
        unsigned int ret = engine_go_where();
        printf("ret is %d\r\n", ret);
        if (ret == CAR_TURN_LEFT) {
            car_left();
            hi_sleep(500);
        } else if (ret == CAR_TURN_RIGHT) {
            car_right();
            hi_sleep(500);
        }
        car_stop();
    } else {
        car_forward();
        } 
}

/*car mode control func*/
static void car_mode_control_func(void)
{
    float m_distance = 0.0;
    regress_middle();
    while (1) {
        if (g_car_status != CAR_OBSTACLE_AVOIDANCE_STATUS) {
            printf("car_mode_control_func 1 module changed\n");
            regress_middle();
            break;
        }

        /*获取前方物体的距离*/
        m_distance = GetDistance();
        car_where_to_go(m_distance);
        hi_sleep(20);
    }
}

void *RobotCarTestTask(void* param)
{
    switch_init();
    interrupt_monitor();

       WifiLink() ;
    
//sjn tcp
    struct sockaddr_in send_addr;
    socklen_t addr_length = sizeof(send_addr);
    char recvBuf[512];

//创建socket
    if ((sock_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
    {
        printf("create socket failed!\r\n");
        exit(1);
    }
//设置为阻塞模式
    int fdopt = setnonblockint(sock_fd);
  printf("create socket ok!\r\n");
 //初始化预连接的服务端地址
    send_addr.sin_family = AF_INET;
    send_addr.sin_port = htons(TCP_PORT);
    send_addr.sin_addr.s_addr = inet_addr(TCP_SERVER_ADRESS);
    addr_length = sizeof(send_addr);

   connectret= connect(sock_fd, (struct sockaddr *)&send_addr, addr_length);
   if((connectret ==-1))
        {
            printf("connect not ok\r\n");
        }

    while (1) {
        
        
        

    if((connectret ==-1))
        {
            printf("connect not ok\r\n");
           connectret=  connect(sock_fd, (struct sockaddr *)&send_addr, addr_length);
             
        }

    //     //发送数据到服务远端
    //     if((ret = send(sock_fd, send_data, strlen(send_data), 0)) == -1)
    //     {
    //         printf("send not ok");
    //     }
    // //car_forward();
    //     //接收服务端返回的字符串
    //     if((ret = recv(sock_fd, recvBuf, sizeof(recvBuf), 0)) == -1)
    //     {
    //         printf("recv error\r\n");
    //     }
    //    // printf("recv:%s\r\n", recvBuf);

        packetheatbeat();
        packetattitude();
        on_messageReceive();
        packetglobal_position();
        IoTWatchDogDisable();
        printf("x:%f/n y:%f/n",x,y);
        if(x==1.0 && y==0)  { 
            car_forward();
            hi_sleep(100);
            car_stop();}
        if(x==-1.0 && y==0)  { //向下按键
            
            car_backward();
            hi_sleep(100);
            car_stop();}
        if(x==1.0 && y==1.0)  {//向右按键
              
             car_right();
            hi_sleep(100);
            car_stop();
         }
        if(x==1.0 && y==-1.0)  { //向左
            car_left();
           
            hi_sleep(100);
            car_stop();}
        osDelay(1);        
    }

        //关闭这个 socket
    closesocket(sock_fd);
}

/**************************************************
 * 任务：sensor_task_entry
 * 传感器采集任务
 * ***********************************************/
static int led_task_entry(void)
{
    hi_io_set_func(9, 0);
    IoTGpioInit(9);

    //设置GPIO_2为输出模式
    IoTGpioSetDir(9, IOT_GPIO_DIR_OUT);

    while (1)
    {
       IoTGpioSetOutputVal(9, 1);
       sleep(1);
       IoTGpioSetOutputVal(9, 0);
       sleep(1);
    }
    return 0;
}

void RobotCarDemo(void)
{
    osThreadAttr_t attr;


    attr.name = "RobotCarTestTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 10240;
    attr.priority = 25;

    if (osThreadNew(RobotCarTestTask, NULL, &attr) == NULL) {
        printf("[Ssd1306TestDemo] Falied to create RobotCarTestTask!\n");
    }
}
APP_FEATURE_INIT(RobotCarDemo);